
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_view.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs_view.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static ahrs_view ahrs;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   ahrs  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_view_update(ahrs_view *ahrs)
{
    uitc_vehicle_origin origin_pos;
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;
    uitc_sensor_acc sensor_acc;
    uitc_sensor_mag sensor_mag;
    uitc_vehicle_alt alt_info;
    uitc_sensor_baro sensor_baro;
    uitc_vehicle_position pos_info;

    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);
    itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag);
    itc_copy_from_hub(ITC_ID(vehicle_alt), &alt_info);
    itc_copy_from_hub(ITC_ID(sensor_baro), &sensor_baro);
    itc_copy_from_hub(ITC_ID(vehicle_position), &pos_info);

    ahrs->roll_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.roll * 100);
    ahrs->pitch_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.pitch * 100);
    ahrs->yaw_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.yaw * 100);

    ahrs->cos_roll = cosf(vehicle_attitude.vehicle_euler.roll);
    ahrs->cos_pitch = cosf(vehicle_attitude.vehicle_euler.pitch);
    ahrs->cos_yaw = cosf(vehicle_attitude.vehicle_euler.yaw);
    ahrs->sin_roll = sinf(vehicle_attitude.vehicle_euler.roll);
    ahrs->sin_pitch = sinf(vehicle_attitude.vehicle_euler.pitch);
    ahrs->sin_yaw = sinf(vehicle_attitude.vehicle_euler.yaw);

    
    ahrs->attitude_body = vehicle_attitude.vehicle_quat;
    
    ahrs->roll = vehicle_attitude.vehicle_euler.roll;
    ahrs->pitch = vehicle_attitude.vehicle_euler.pitch;
    ahrs->yaw = vehicle_attitude.vehicle_euler.yaw;
    
    ahrs->gyr.x = sensor_gyr.sensor_gyr_correct[0];
    ahrs->gyr.y = sensor_gyr.sensor_gyr_correct[1];
    ahrs->gyr.z = sensor_gyr.sensor_gyr_correct[2];

    ahrs->acc.x = sensor_acc.sensor_acc_correct[0];
    ahrs->acc.y = sensor_acc.sensor_acc_correct[1];
    ahrs->acc.z = sensor_acc.sensor_acc_correct[2];
    
    ahrs->accel_ef.x = pos_info.ax;
    ahrs->accel_ef.y = pos_info.ay;
    ahrs->accel_ef.z = -alt_info.az - GRAVITY_MSS;

    ahrs->relpos_cm.x = pos_info.x*100.0f;
    ahrs->relpos_cm.y = pos_info.y*100.0f;
    ahrs->relpos_cm.z = alt_info.relative_alt * 100.0f;

    ahrs->velocity_cm.x = pos_info.vx*100.0f;
    ahrs->velocity_cm.y = pos_info.vy*100.0f;
    ahrs->velocity_cm.z = alt_info.vz * 100.0f;

    if (itc_copy_from_hub(ITC_ID(vehicle_origin), &origin_pos) == 0) {
        location_from_lat_lon_alt(&ahrs->curr_loc, origin_pos.lat, origin_pos.lon, alt_info.relative_alt*100, ALT_FRAME_ABSOLUTE);
        location_offset(&ahrs->curr_loc, pos_info.x, pos_info.y);
    }
}

/*------------------------------------test------------------------------------*/


